#include "fishbot_tf_republish/map_odom_republish.h"

void RepublishMapOdom::amclCallBack(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
    amcl_pose_ = *msg;
    RCLCPP_INFO(this->get_logger(), "Received AMCL pose");
}

void RepublishMapOdom::odomCallBack(const nav_msgs::msg::Odometry::SharedPtr msg)
{
    odom_ = *msg;
    RCLCPP_INFO(this->get_logger(), "Received Odom");
}


void RepublishMapOdom::rePublish()
{
    // 检查 amcl_pose_ 是否已接收到数据
    if (amcl_pose_.header.frame_id.empty())
    {
        RCLCPP_WARN(this->get_logger(), "AMCL pose not received yet, cannot publish map->odom transform.");
        return;
    }
    geometry_msgs::msg::TransformStamped geomsg_odom_base_link;
    // 获取 base_link 到 tag 的变换
    try{
     geomsg_odom_base_link = tf_buffer_->lookupTransform(
        "odom",
        "base_footprint",
        amcl_pose_.header.stamp,
        rclcpp::Duration::from_nanoseconds(100000));
    }
    catch (const tf2::TransformException &ex)
    {
        RCLCPP_WARN(this->get_logger(), "Failed to get odom to base_link transform: %s", ex.what());
        return;
    }

    // 将amcl_pose_(map->base_link的变换)转换为 tf2::Transform
    tf2::Transform tf_map_base_link;
    tf_map_base_link.setOrigin(tf2::Vector3(amcl_pose_.pose.pose.position.x,
                                            amcl_pose_.pose.pose.position.y,
                                            amcl_pose_.pose.pose.position.z));
    tf_map_base_link.setRotation(tf2::Quaternion(amcl_pose_.pose.pose.orientation.x,
                                                 amcl_pose_.pose.pose.orientation.y,
                                                 amcl_pose_.pose.pose.orientation.z,
                                                 amcl_pose_.pose.pose.orientation.w));
    // 将 tf_odom_base_link_ 转换为 tf2::Transform
    tf2::Transform tf_odom_base_link;
    tf_odom_base_link.setOrigin(tf2::Vector3(geomsg_odom_base_link.transform.translation.x,
                                             geomsg_odom_base_link.transform.translation.y,
                                             geomsg_odom_base_link.transform.translation.z));
    tf_odom_base_link.setRotation(tf2::Quaternion(geomsg_odom_base_link.transform.rotation.x,
                                                  geomsg_odom_base_link.transform.rotation.y,
                                                  geomsg_odom_base_link.transform.rotation.z,
                                                  geomsg_odom_base_link.transform.rotation.w));
    
    // 根据tf_map_to_base_link和tf_odom_base_link_计算map->odom的变换
    tf2::Transform tf_map_to_odom = tf_map_base_link * tf_odom_base_link.inverse();

    // 将变换转换为 geometry_msgs::msg::TransformStamped
    geometry_msgs::msg::TransformStamped map_to_odom;
    map_to_odom.transform.translation.x = tf_map_to_odom.getOrigin().x();
    map_to_odom.transform.translation.y = tf_map_to_odom.getOrigin().y();
    map_to_odom.transform.translation.z = tf_map_to_odom.getOrigin().z();
    map_to_odom.transform.rotation.x = tf_map_to_odom.getRotation().x();
    map_to_odom.transform.rotation.y = tf_map_to_odom.getRotation().y();
    map_to_odom.transform.rotation.z = tf_map_to_odom.getRotation().z();
    map_to_odom.transform.rotation.w = tf_map_to_odom.getRotation().w();
    map_to_odom.header.frame_id = "map";
    map_to_odom.child_frame_id = "odom";
    map_to_odom.header.stamp = amcl_pose_.header.stamp;

    // 将变换封装到 TFMessage 中
    tf2_msgs::msg::TFMessage tf_map_odom;
    tf_map_odom.transforms.push_back(map_to_odom);
    // if (odom_.twist.twist.angular.z < 0.5)
    // 发布 TF 变换
    pub_tf_->publish(tf_map_odom);

    RCLCPP_INFO(this->get_logger(), "Published map->odom transform.");
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RepublishMapOdom>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}